#include "SimpleSerial.cpp"
#include <iostream>
#include <cstdio>
#include <ros/ros.h>
#include <ros/console.h>
#include <string>
#include <iostream>
#include <time.h>
#include <fstream>
#include <stdlib.h>
#include <math.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
using namespace std;

#define DEFAULT_SPEED_LEFT 1500
#define DEFAULT_SPEED_RIGHT 1500
#define LEFT 0
#define RIGHT 1

typedef char byte;

class MotorCommander {
 public:
  MotorCommander(int side, const char* serial_port, int baud)
      : side_(side),
	current_speed_(1000), // start without motor movement
        serial_(serial_port, baud) {
  }

  void commandMotors(short command) {
    // log any errors about incorrect command speeds
    if (command < 0 || command > 2000) {
      cerr << "ERROR: Commanding motor to move at speed "
	   << command << "\n";
    }

    // Set the current speed of this side's motors
    current_speed_ = command;
    short lower_payload = command;
    short upper_payload = command;
  
    // Craft the upper and lower payloads
    lower_payload = lower_payload & 0x003F;
    upper_payload = upper_payload >> 6;
    upper_payload = upper_payload & 0x003F;
  
    /*
     * Determine which side we're on and adjust the bytes accordingly.
     *
     * If this is the left motor commander, the upper payload gets header
     * 00 and the lower payload gets 01.
     *
     * If this is the right motor commander, the upper payload gets header
     * 10 and the lower payload gets 11.
     */
    //printf("Have %x %x so far!\n", upper_payload, lower_payload);
    if (side_ == 0) {
      lower_payload | 0x40;
    } else {
      upper_payload | 0x80;
      lower_payload | 0xC0;
    }
    byte b1 = (byte)lower_payload;
    byte b0 = (byte)upper_payload;
   //printf("Writing %x %x to the motor!\n", b0, b1);

    serial_.serial_write(&b0, 1);
    serial_.serial_write(&b1, 1);
  }

  int getCurrentSpeed() {
    return current_speed_;
    //cout << "Current speed is "<< current_speed_ << "\n";
  }

  
 private:
  int side_; // 0 for left, 1 for right
  int current_speed_; // 0 to 2000, 1000 is neutral
  SimpleSerial serial_;
  
  MotorCommander() {};

};

class DemoMower {
 public:
  DemoMower(const char* serial_port, int baud)
    : right_motors_(RIGHT, serial_port, baud), 
      left_motors_(LEFT, serial_port, baud) {
  }
  
  void ControlledMove(int speed) {
    int right_difference = speed - right_motors_.getCurrentSpeed();
    int left_difference = speed - left_motors_.getCurrentSpeed();
    int right_increment = right_difference / 3;
    int left_increment = left_difference / 3;
 
    for (int i = 0; i < 3; i++) {
      left_motors_.commandMotors(left_motors_.getCurrentSpeed()+left_increment);
      right_motors_.commandMotors(right_motors_.getCurrentSpeed()+right_increment);
      system("sleep 0.1");
    }
  }


  void ControlledTurnLeft(int speed) {
    int difference = speed - right_motors_.getCurrentSpeed();
    int increment = difference / 10;
    for (int i = 0; i < 10; i++) {
      left_motors_.commandMotors(left_motors_.getCurrentSpeed()+increment);
      right_motors_.commandMotors(right_motors_.getCurrentSpeed()-increment);
      system("sleep 0.1");
    }
  } 

  void ControlledTurnRight(int speed) {
  int difference = speed - left_motors_.getCurrentSpeed();
  int increment = difference / 10;
  for (int i = 0; i < 10; i++) {
      left_motors_.commandMotors(left_motors_.getCurrentSpeed()-increment);
      right_motors_.commandMotors(right_motors_.getCurrentSpeed()+increment);
      system("sleep 0.1");
    }
  } 

  void ControlledVeerLeft(int adjustment) {
    int increment = adjustment / 1;
    for (int i = 0; i < 1; i++) {
      ROS_INFO("Increment is %d", increment);
      left_motors_.commandMotors(DEFAULT_SPEED_RIGHT+increment);
      right_motors_.commandMotors(DEFAULT_SPEED_LEFT-increment);
      ROS_INFO("LEFT VEER Left motors now %d Right motors now %d",
	       left_motors_.getCurrentSpeed(), right_motors_.getCurrentSpeed());
    }
  }


  void ControlledVeerRight(int adjustment) {
    int increment = adjustment / 1;
    for (int i = 0; i < 1; i++) {
      left_motors_.commandMotors(DEFAULT_SPEED_RIGHT-increment);
      right_motors_.commandMotors(DEFAULT_SPEED_LEFT+increment);
      ROS_INFO("RIGHT VEER Left motors now %d Right motors now %d",
	       left_motors_.getCurrentSpeed(), right_motors_.getCurrentSpeed());
    }
  }

  void stop() {
    left_motors_.commandMotors(1000);
    right_motors_.commandMotors(1000);
  }

  void WhatIsCurrentSpeed() {
    ROS_INFO("Left motors now %d Right motors now %d",
	     left_motors_.getCurrentSpeed(), right_motors_.getCurrentSpeed());
  }
 
  private:
  MotorCommander right_motors_;
  MotorCommander left_motors_;
};
